%**************************************************************************
%BatBot: Biological inspired Bat roBot.

%Copyright RObotics and Cybernetics Group
%Julian Colorado

% Matlab simulator of bat flight behavior. 
%**************************************************************************

%Compute Forward Dynamics using Inverse dynamics to obtain mass operator
%This program returns the joint acceleration

%Entrance parameters:

%DHC:   alpha  a teta  d  sigma  m  Ixx  Iyy  Izz  sx  sy  sz
%Initial joint trajectory: Q,dQ

%**************************************************************************
function [d2Q,Vel,Pos]=forw_dyn_JDC(DHC,Torque,Q,dQ,Time)

 [pt,n]=size(Torque);
 %Obtain mass operator defining V=0 and A=eye(n) without gravity effect computing Inverse Dynamics:
 for k=1:pt
  [Mass,temp] = inv_dyn(n,DHC,ones(n,1)*Q(1,:),zeros(n,n),eye(n),[0 0 0 0 0 0]',[0 0 0 0 0 0]');
 %Compute Inverse dynamics to eliminate terms dependens of velocity
  [tv,temp] = inv_dyn(n,DHC,Q(1,:),dQ(1,:),zeros(1,n),[0 0 0 0 0 0]',[0 0 0 0 0 9.81]');
  d2Q(k,:) = inv(Mass)*(Torque(k,:)-tv)';
 end

%Verifying if computed-forces generate the desired motion of the system
%Integrating Acceleration using simple EULER V(i+1)=V(i)*step*A(i) in order to 
%obtain velocities and positions

Vel(1,1:n) = dQ(1,:); %Initial vel
 for k=1:pt-1
     step = Time(k+1)-Time(k);
     Vel(k+1,:) = Vel(k,:)+(d2Q(k,:)*step);     
 end

[pt,n]=size(Vel);
Pos(1,1:n) = Q(1,:); %Initial Pos
  for k=1:pt-1
     Pos(k+1,:) = Pos(k,:)+(Vel(k,:)*step); 
  end

  
end



  